#pragma once
#include "../DirectX/stdafx.h"
#include "Blind_PathFinding_Plane.h"
#include "BariersLevel.h"
#include "../DirectX/Direct_All.h"

#include "rec/robotino/com/all.h"
#include "rec/core_lt/utils.h"
#include "rec/robotino/com/DistanceSensor.h"

using namespace rec::robotino::com;


class Physix_Location
{
private:
	vector<rec::robotino::com::DistanceSensor>				Sensors;
	vector<D3DRectangle>				Lasers;
	vector<D3DXVECTOR4>					dirs,
										dirs2;
	D3DXVECTOR3							center;

	LPDIRECT3DVERTEXBUFFER9				g_pVB;

	LPDIRECT3DTEXTURE9					no_detected,
										detected;

	D3DXMATRIX							Matrix;

	Blind_PathFinding_Plane				Path;

	time_t								wait;

	float								sensativity;

public:
	BariersLevel						Bariers;

	vector<float>						buf;

	Physix_Location(){
		sensativity = 0;}
	Physix_Location(LPDIRECT3DDEVICE9 &pd3dDevice, D3DXVECTOR3 &center_)
	{
		g_pVB	=	NULL;

		center	=	D3DXVECTOR3(center_);

		Sensors.	resize(9);
		dirs.		resize(9);
		dirs2.		resize(9);
		Lasers.		resize(9);
		buf.		resize(9, 100);

		D3DXMATRIX						Rotate;
		D3DXMatrixIdentity				(&Rotate);
		D3DXMatrixIdentity				(&Matrix);

		D3DXCreateTextureFromFile(pd3dDevice, L"UI_textures/no_detected.png",	&no_detected);
		D3DXCreateTextureFromFile(pd3dDevice, L"UI_textures/detected.png",		&detected);
		
		for(float q=0; q<buf.size(); q++)
		{
			D3DXMatrixRotationZ(&Rotate, q * 0.69813170079f );

			D3DXVec3Transform(&dirs[q],  &D3DXVECTOR3(0, 1, 0), &Rotate); 
			D3DXVec3Transform(&dirs2[q], &D3DXVECTOR3(1, 0, 0), &Rotate); 
		}

		sensativity	=	0;

		SetupVB(pd3dDevice);
	}

	void SetupVB(LPDIRECT3DDEVICE9 &pd3dDevice)
	{
		UINT size = Lasers.size();
			 size *= 6;
		CUSTOMVERTEX *g_Vertices = new CUSTOMVERTEX[size];

		for(UINT q=0; q<Lasers.size(); q++)
			memcpy(g_Vertices + (6*q),						Lasers[q].init(),	6*sizeof(CUSTOMVERTEX));

		if(g_pVB != NULL)
			g_pVB->Release();

		pd3dDevice->CreateVertexBuffer(
			size * sizeof(CUSTOMVERTEX),
			D3DUSAGE_DYNAMIC,
			D3DFVF_CUSTOMVERTEX,
			D3DPOOL_DEFAULT,
			&g_pVB,
			NULL );

		VOID* pVertices;

		g_pVB->Lock( 0, size * sizeof(CUSTOMVERTEX), (void**)&pVertices, 0 );

		memcpy( pVertices, g_Vertices, size * sizeof(CUSTOMVERTEX) );
		g_pVB->Unlock();


		delete g_Vertices,pVertices;
	}

	void UpdateSensors(LPDIRECT3DDEVICE9 &pd3dDevice, float R, float width, float length)
	{
		if(clock() < wait)
			return;

		wait	=	clock() + 50;

		vector<float>		voltages(9);

		for(int q=0; q<buf.size(); q++)
		{
			float buffer =  Sensors[q].voltage();

			buf[q] = 2.5f - (buf[q] + buffer) / 2;

			voltages[q]  =  buf[q];

			D3DXVECTOR3			dir (dirs[q].x,  dirs[q].y,  dirs[q].z ),
								dir2(dirs2[q].x, dirs2[q].y, dirs2[q].z);

			D3DXVECTOR3			p4	=	Math::GetPoint_Vector_Length(center, dir, R/2);
								p4	=	Math::GetPoint_Vector_Length(p4, dir2,	-width/2);

			D3DXVECTOR3			p2	=	Math::GetPoint_Vector_Length(p4, dir2, width),
								p1	=	Math::GetPoint_Vector_Length(p4, dir, buf[q] * length),
								p3	=	Math::GetPoint_Vector_Length(p2, dir, buf[q] * length);

			Lasers[q] = D3DRectangle(p1, p2, p3, p4);			
		}
		Path.Update(voltages);

		SetupVB(pd3dDevice);
	}

	bool Render(LPDIRECT3DDEVICE9 &pd3dDevice)
	{
		pd3dDevice->SetFVF				( D3DFVF_CUSTOMVERTEX );
		pd3dDevice->SetStreamSource		( 0, g_pVB, 0, sizeof(CUSTOMVERTEX));

		D3DXMatrixRotationZ(&Matrix, sin( (float)  (clock()  % 31415) / 1000 ) / 6 );

		pd3dDevice->SetTransform		(D3DTS_WORLD, &Matrix);

		for(UINT q=0; q<buf.size(); q++)
		{
			if(buf[q] < sensativity)
			{
				if(FAILED(pd3dDevice->SetTexture	(0,	detected)))					return false;
			}
			else
				if(FAILED(pd3dDevice->SetTexture	(0,	no_detected)))				return false;

			if(FAILED(pd3dDevice->DrawPrimitive	(D3DPT_TRIANGLELIST, q*6, 2)))		return false;
		}

		return true;
	}

	bool Detected(int index)
	{
		return buf[index] < sensativity;
	}

	void setSensativity(float newSensativity)
	{
		sensativity = newSensativity;
	}
	float getSensativity()
	{
		return sensativity;
	}

	vector<D3DXVECTOR4>*	getDirs()
	{
		return &dirs;
	}

	void Connect(rec::robotino::com::ComId &ID)
	{
		Sensors.resize(rec::robotino::com::DistanceSensor::numDistanceSensors());
		for(int q=0;q<Sensors.size();q++)
		{
			Sensors[q].setSensorNumber(q);
			Sensors[q].setComId(ID);
		}
	}
};